#include <geometry_msgs/TwistWithCovarianceStamped.h> // gps/vel
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>
#include <nav_msgs/Odometry.h> // gps/odom
#include <pcl/point_types.h>
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h> // gps/fix
#include <sensor_msgs/PointCloud2.h>
#include <xyz_lla.h>
#include <iostream>

void format_output() {
  std::cout.setf(std::ios::scientific);
  std::cout.precision(11);
}

void normal_output() {
  std::cout.unsetf(std::ios::scientific);
  std::cout.precision(6);
}

void callback(const sensor_msgs::NavSatFixConstPtr &gps_fix,
              const nav_msgs::OdometryConstPtr &gps_odom,
              const geometry_msgs::TwistWithCovarianceStampedConstPtr &gps_vel,
              const sensor_msgs::PointCloud2ConstPtr &untransed_pointcloud) {

  pcl::PointXYZ xyz{28, 4, -1.6};
  Angle angle;

  format_output();
  Xyz_trans pos(*gps_fix, *gps_vel, *gps_odom, xyz);
  normal_output();

  angle = pos.CalculateAngle();
  std::cout << "当前车体sin值:" << angle.sin << ",cos值:" << angle.cos
            << std::endl;

  LogLatAlt transfered_lla{0, 0, 0};

  format_output();
  transfered_lla = pos.CalculateLla();
  std::cout << "转换后的经纬高(" << transfered_lla.log << ","
            << transfered_lla.lat << "," << transfered_lla.alt << ")"
            << std::endl;
  normal_output();

  std::cout << "--------转换完毕--------" << std::endl;
}

void callback_tmp(
				const sensor_msgs::NavSatFixConstPtr &gps_fix,
				const nav_msgs::OdometryConstPtr &gps_odom,
				const geometry_msgs::TwistWithCovarianceStampedConstPtr &gps_vel) {

  pcl::PointXYZ xyz{28, 4, -1.6};
  Angle angle;

  Xyz_trans pos(*gps_fix, *gps_vel, *gps_odom, xyz);
  format_output();
  pos.CurrentPosShow();
  normal_output();

  angle = pos.CalculateAngle();
  std::cout << "当前车体sin值:" << angle.sin << ",cos值:" << angle.cos
            << std::endl;

  LogLatAlt transfered_lla{0, 0, 0};

  format_output();
  transfered_lla = pos.CalculateLla();

  //  std::cout << "相对正北角度:" << north_angle
  //            << ",读取的车体sin值:" << std::sin(north_angle)
  //            << ",cos值:" << std::cos(north_angle) << std::endl;

  std::cout << "转换后的经纬高(" << transfered_lla.log << ","
            << transfered_lla.lat << "," << transfered_lla.alt << ")"
            << std::endl;
  normal_output();

  std::cout << "--------转换完毕--------" << std::endl;
}

int main(int argc, char **argv) {

  ros::init(argc, argv, "xyz_lla_node");
  ros::Time::init();
  ros::NodeHandle nh;
  // 时间同步
  ROS_INFO("start message filter");
  message_filters::Subscriber<sensor_msgs::NavSatFix> gps_fix_sub(
      nh, "/gps/fix", 1);
  message_filters::Subscriber<nav_msgs::Odometry> gps_odom_sub(nh, "/gps/odom",
                                                               1);
  message_filters::Subscriber<geometry_msgs::TwistWithCovarianceStamped>
      gps_vel_sub(nh, "/gps/vel", 1);
  message_filters::Subscriber<sensor_msgs::PointCloud2>
      untransed_pointcloud_sub(nh, "/untransed_pointcloud", 1);

  typedef message_filters::sync_policies::ApproximateTime<
      sensor_msgs::NavSatFix, nav_msgs::Odometry,
      geometry_msgs::TwistWithCovarianceStamped, sensor_msgs::PointCloud2>
      SyncPolicy;

  message_filters::Synchronizer<SyncPolicy> topic_time_sync(
      SyncPolicy(10), gps_fix_sub, gps_odom_sub, gps_vel_sub,
      untransed_pointcloud_sub);

  //		topic_time_sync.registerCallback(boost::bind(&callback,_1,_2,_3,_4));

  // -----------------------------test----------------------------
  typedef message_filters::sync_policies::ApproximateTime<
				  sensor_msgs::NavSatFix,nav_msgs::Odometry,geometry_msgs::TwistWithCovarianceStamped
				  > SyncPolicy_tmp;

  message_filters::Synchronizer<SyncPolicy_tmp> topic_time_sync_tmp(
      SyncPolicy_tmp(10), gps_fix_sub,gps_odom_sub,gps_vel_sub);

  topic_time_sync_tmp.registerCallback(boost::bind(&callback_tmp, _1, _2, _3));

  ros::Rate r = ros::Rate(200);
  // 时间同步结束
  while (ros::ok()) {
    ros::spin();
  }

  return 0;
}
